//
// Created by sri01 on 2024/7/20.
//
#include "MT6816.h"
#include "d_math.h"
#include "math.h"

// GPIO输出
#define MT6816_CS_H() HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET);
#define MT6816_CS_L() HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_RESET);

#define DIR      1
#define TIME_OUT (1000U)

float angle1 = 0, angle2 = 0;

HAL_StatusTypeDef HAL_Status;

/**
 * @brief  MT6816_SPI采集获取角度数据
 * @param  NULL
 * @return NULL
 **/
void MT6816_Get_MagData(MT6816_t* MT6816)
{
    uint16_t temp = 0;
    uint16_t data_t[4];
    uint16_t data_r[4];
    uint8_t h_count;
    data_t[0] = (0x80 | 0x03) << 8;
    data_t[1] = (0x80 | 0x04) << 8;
    for(uint8_t i = 0; i < 3; i++)
    {
        // 读取SPI数据
        MT6816_CS_L();
        HAL_Status |= BSP_SPI_TransmitReceive(&hspi1, (uint8_t*)&data_t[0], (uint8_t*)&data_r[0], 1, TIME_OUT);
        MT6816_CS_H();

        MT6816_CS_L();
        HAL_Status |= BSP_SPI_TransmitReceive(&hspi1, (uint8_t*)&data_t[1], (uint8_t*)&data_r[1], 1, TIME_OUT);
        MT6816_CS_H();

        temp = ((data_r[0] & 0x00FF) << 8) | (data_r[1] & 0x00FF);
        // 奇偶校验
        h_count = 0;
        for(uint8_t j = 0; j < 16; j++)
        {
            if(temp & (0x0001 << j))
                h_count++;
        }
        if(h_count & 0x01)
        {
            MT6816->real_flag = false;
        }
        else
        {
            MT6816->real_flag = true;
            break;
        }
    }
    if(MT6816->real_flag)
    {
        MT6816->initial_data = temp >> 2;
        MT6816->mag_flag = (bool)(temp & (0x0001 << 1));
    }

    angle2 = angle1;
    angle1 = MT6816->initial_data;
    float data1;
    if(fabsf(angle1 - angle2) < 8000)
    {
        data1 = (angle1 - angle2) / 16384;
    }
    else
    {
        if(angle1 - angle2 > 8000)
        {
            data1 = (angle1 - angle2 - 16384) / 16384;
        }
        if(angle1 - angle2 < -8000)
        {
            data1 = (angle1 - angle2 + 16384) / 16384;
        }
        MT6816->rounds++;
    }
    MT6816->rps = data1 * 2 * M_PI * 20000;
    MT6816->rpm = MT6816->rps * 60;
}

/**
 * @brief  MT6816获取角度数据
 * @param  NULL
 * @retval NULL
 **/
#define ElAngle_offset 0
void GetFocMegAngle(MT6816_t* MT6816)
{
    MT6816_Get_MagData(MT6816);
    MT6816->Encoder.MecAngle = (float)((MT6816->initial_data + ElAngle_offset)&0x4fff) * M_PI / 8192.0f;
    MT6816->Encoder.MecSpeed = MT6816->rpm;
    MT6816->Encoder.ElAngle = fmodf(MT6816->Encoder.MecAngle * MOTOR_P, M_2PI);
}
